package com.wpi.smarttour;
import java.io.BufferedReader;
import java.io.FileReader;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
import java.util.StringTokenizer;
import com.wpi.smarttour.helpers.Utilities;
import android.os.Environment;

/*
 * Class used to Snap to Grid and Find shorted Path
 */
public class PathFinder
{
   int selected_index = -1; // index of selected building
   int currentX = 0;
   int currentY = 0;
   int closestX = 0; //x position calculated to be closest point on line
   int closestY = 0; //y position calculated to be closest point on line
   double currentSlope = 0;
   int[][] segments; // array of endpoints for all Edges
   ArrayList<Node> nodes; // all Nodes used to define campus paths
   ArrayList<Edge> edges; // all Edges between nodes
   ArrayList<Node> path; // will contain path to destination node
   HashMap<String, Integer> buildingEntrances;  //all buildings entrances, keys are building names

   public PathFinder()
   {
      readWaypointsFromCSV(); //initialize nodal data
      createEdges();          //instantiate paths
      Utilities.LogDebug("PathFinder", "Initializing");
   }

   //returns the closest X,Y pair on the closest line segment   
   public int[] getClosestSegment(int x, int y)
   {
      currentX = x;
      currentY = y;
      int placeholder = 0;
      int xBound1 = 0;
      int xBound2 = 0;
      int yBound1 = 0;
      int yBound2 = 0;
      double sideAB = 10000;
      double sideAC = 10000;
      double sideBC = 10000;
      double closestSideAC = 10000;
      double closestSideBC = 10000;
      double s;
      double area;
      double distance = 10000;
      double closestDistance = 10000;
      double secondClosestPointDistance = 10000;
      double secondPointDistance = 10000;
      double[] lineVariables;
      boolean box = true;
      boolean inBounds = false;
      boolean closestInBounds = true;
      int closestIndex = 0;
      int[] closestCoords = {0, 0};
      //loop through all line segments
      for(int i = 0; i < segments.length; i++)
      {
         s = area = -1;
         lineVariables = solveForLineEquations(i); // returns slope, 1st
                                                   // y-intercept, 2nd intercept
         //determine the bounds within which the triange formula can be used
         xBound1 = evaluateLineEquation(lineVariables[0], lineVariables[1], y, 'y');
         xBound2 = evaluateLineEquation(lineVariables[0], lineVariables[2], y, 'y');
         yBound1 = evaluateLineEquation(lineVariables[0], lineVariables[1], x, 'x');
         yBound2 = evaluateLineEquation(lineVariables[0], lineVariables[2], x, 'x');
         //switch x,y bounds if needed, to maintain proper max/min
         if (xBound1 > xBound2)
         {
            placeholder = xBound2;
            xBound2 = xBound1;
            xBound1 = placeholder;
         }
         if (yBound1 > yBound2)
         {
            placeholder = yBound2;
            yBound2 = yBound1;
            yBound1 = placeholder;
         }
         //use the Pythagorean theorem to find the three sides of the triangle
         sideAB = Math.sqrt(Math.pow(segments[i][0] - segments[i][2], 2) + Math.pow(segments[i][1] - segments[i][3], 2));
         sideAC = Math.sqrt(Math.pow(segments[i][0] - x, 2) + Math.pow(segments[i][1] - y, 2));
         sideBC = Math.sqrt(Math.pow(segments[i][2] - x, 2) + Math.pow(segments[i][3] - y, 2));
         //if the user's location is within the x,y bounds for the triangle formula, do calculations
         if (x >= xBound1 && x <= xBound2 && y >= yBound1 && y <= yBound2)
         {
            s = (sideAB + sideAC + sideBC) / 2;
            area = Math.sqrt(s * (s - sideAB) * (s - sideAC) * (s - sideBC));
            distance = (2 * area / sideAB);
            inBounds = true;
         }
         else
         {
            //otherwise, determine which end node is closer
            if (sideAC < sideBC)
            {
               distance = sideAC;
               secondPointDistance = sideBC;
            }
            else
            {
               distance = sideBC;
               secondPointDistance = sideAC;
            }
            inBounds = false;
         }
         if (i == 0)
         {
            //make sure sure to initialize closestDistance
            closestDistance = distance;
         }
         if (distance < closestDistance)
         {
            //if the new distance from the current segment is the shortest so far
            //do the calculations to store it
            closestIndex = i;
            closestDistance = distance;
            closestInBounds = inBounds;
            box = inBounds;
            closestSideAC = sideAC;
            closestSideBC = sideBC;
         }
         else if (distance == closestDistance && secondPointDistance < secondClosestPointDistance)
         {
            //if the distance to the end point is the same as before
            //but the rest of the line segment is closer, store it
            box = inBounds;
            closestIndex = i;
            secondClosestPointDistance = secondPointDistance;
            closestInBounds = inBounds;
            closestSideAC = sideAC;
            closestSideBC = sideBC;
         }
      }
      if (closestInBounds)
      {
         //if the point is within bounds, get the closest point on the line
         closestCoords = returnClosestPoint(x, y, closestIndex);
      }
      else if (closestSideAC < closestSideBC)
      {
         //otherwise, store the nearest point as node 'A'
         closestCoords[0] = segments[closestIndex][0];
         closestCoords[1] = segments[closestIndex][1];
      }
      else
      {
         //or store it is node 'B', whichever is closer
         closestCoords[0] = segments[closestIndex][2];
         closestCoords[1] = segments[closestIndex][3];
      }
      closestX = closestCoords[0];
      closestY = closestCoords[1];
      return new int[]{closestX, closestY};
   }

   //given a line segment, determine what is the closest point
   //on that line segment to the given <X,Y> pair
   public int[] returnClosestPoint(int x, int y, int whichLine)
   {
      //calculate slope
      double rise = segments[whichLine][1] - segments[whichLine][3];
      double run = segments[whichLine][0] - segments[whichLine][2];
      double slope = 0;
      double pSlope = 0;
      double intercept = 0;
      double pIntercept = 0;
      double intersectX = 0;
      double intersectY = 0;
      //check for zero and infinite slopes
      if (run != 0 && rise != 0 && run != (-0) && rise != (-0))
      {
         //if the case is checked, calculate the line intersections
         slope = rise / run;
         pSlope = -1 / slope;
         intercept = segments[whichLine][1] - slope * segments[whichLine][0];
         pIntercept = y - pSlope * x;
         intersectX = ((intercept - pIntercept) / (pSlope - slope));
         intersectY = (pSlope * intersectX + pIntercept);
      }
      else if (rise == 0 || rise == (-0))
      {
         //some weird quick requires checking for negative zero
         //deals with slope=0
         slope = 0;
         pSlope = 100000;
         intersectX = x;
         intersectY = segments[whichLine][1];
      }
      else if (run == 0 || run == (-0))
      {
         //some weird quick requires checking for negative zero
         //deals with slope=infinity
         slope = 100000;
         pSlope = 0;
         intersectX = segments[whichLine][0];
         intersectY = y;
      }
      return new int[]{(int) intersectX, (int) intersectY};
   }

   //solves for the equations of the two perpendicular lines
   //that pass through the endpoints of a line segment
   //used to check for bounds by the triangle area method
   public double[] solveForLineEquations(int whichLine)
   {
      //calculate slopes
      double rise = segments[whichLine][1] - segments[whichLine][3];
      double run = segments[whichLine][0] - segments[whichLine][2];
      double slope = 0;
      if (run != 0)
      {
         slope = rise / run;
         slope = -1 / (slope);
      }
      if (slope > 1000000)
      {
         slope = 1000000;
      }
      //find y-intercepts
      double yIntercept1 = segments[whichLine][1] - slope * segments[whichLine][0];
      double yIntercept2 = segments[whichLine][3] - slope * segments[whichLine][2];
      //return the shared slope of the two lines and their intercepts
      //this is enough information to reconstruct two y=mx+b line equations
      double[] values = new double[]{slope, yIntercept1, yIntercept2};
      return values;
   }

   //takes in the coefficients for a y=mx+b line equations
   //and evaluates it for either 'X' or 'Y'
   public int evaluateLineEquation(double slope, double intercept, int coord, char xOrY)
   {
      int newCoord = -1;
      if (xOrY == 'x')
      {
         newCoord = (int) (slope * coord + intercept);
      }
      else if (xOrY == 'y')
      {
         newCoord = (int) ((coord - intercept) / slope);
      }
      return newCoord;
   }

   //initializes the paths between nodes
   public void createEdges()
   {
      //create an empty ArrayList, scroll through the segments array,
      //which contains the endpoints of all segments
      edges = new ArrayList<Edge>();
      for(int i = 0; i < segments.length; i++)
      {
         //create a new blank Edge objects
         Edge e = new Edge();
         for(int j = 0; j < nodes.size(); j++)
         {
            //and slowly fill in its data, as it finds Node objects
            //whose endpoints match the coordinates from the segments array 
            Node temp = nodes.get(j);
            if (temp.x == segments[i][0] && temp.y == segments[i][1])
            {
               e.to = temp;
            }
            else if (temp.x == segments[i][2] && temp.y == segments[i][3])
            {
               e.from = temp;
            }
         }
         //assign a weight to the path, using the Pythagorean Theorem
         e.weight = (int) Math.sqrt(Math.pow(e.from.x - e.to.x, 2) + Math.pow(e.from.y - e.to.y, 2));
         edges.add(e);
      }
   }

   //performs the calculations for Dijkstra's algorithm
   //which determines the shortest path from one node to another
   //adapted from http://algowiki.net/wiki/index.php?title=Dijkstra's_algorithm
   //assumes Nodes are numbered 0, 1, ... n
   public ArrayList<Node> findShortestPath(int sourceIndex, int targetIndex)
   {
      Node source = nodes.get(sourceIndex);
      Node target = nodes.get(targetIndex);
      int[][] Weight = initializeWeight();
      int[] D = new int[nodes.size()];
      Node[] P = new Node[nodes.size()];
      ArrayList<Node> C = new ArrayList<Node>();
      // initialize:
      // (C)andidate set,
      // (D)yjkstra special path length, and
      // (P)revious Node along shortest path
      for(int i = 0; i < nodes.size(); i++)
      {
         C.add(nodes.get(i));
         D[i] = Weight[source.name][i];
         if (D[i] != Integer.MAX_VALUE)
         {
            P[i] = source;
         }
      }
      // crawl the graph
      for(int i = 0; i < nodes.size() - 1; i++)
      {
         // find the lightest Edge among the candidates
         int l = Integer.MAX_VALUE;
         Node n = source;
         // find the node with the shortest distance away
         for(Node j: C)
         {
            if (D[j.name] < l)
            {
               n = j;
               l = D[j.name];
            }
         }
         C.remove(n);
         // see if any Edges from this Node yield a shorter path than from
         // source->that Node
         for(int j = 0; j < nodes.size(); j++)
         {
            if (D[n.name] != Integer.MAX_VALUE && Weight[n.name][j] != Integer.MAX_VALUE
                  && D[n.name] + Weight[n.name][j] < D[j])
            {
               // found one, update the path
               D[j] = D[n.name] + Weight[n.name][j];
               P[j] = n;
            }
         }
      }
      // we have our path. reuse C as the result list
      C.clear();
      int loc = target.name;
      C.add(target);
      // backtrack from the target by P(revious), adding to the result list
      while(P[loc] != source)
      {
         if (P[loc] == null)
         {
            // looks like there's no path from source to target
            return null;
         }
         C.add(0, P[loc]);
         loc = P[loc].name;
      }
      C.add(0, source);
      return C;
   }

   //Sets up the weighting for each of the paths in a two-dimensional array
   //A two-dimensional array is important for ease of computation for Dijkstra's
   //adapted from http://algowiki.net/wiki/index.php?title=Dijkstra's_algorithm
   private int[][] initializeWeight()
   {
      int[][] Weight = new int[nodes.size()][nodes.size()];
      for(int i = 0; i < nodes.size(); i++)
      {
         //Initialize all weights to max integer value, specifying no path 
         Arrays.fill(Weight[i], Integer.MAX_VALUE);
         //set all self-directing paths to 0 distance 
         Weight[i][i] = 0;
      }
      for(Edge e: edges)
      {
         //Assign the weight from each edge into the array
         Weight[e.from.name][e.to.name] = e.weight;
         //be sure to mirror the path weights, 'A'->'B' = 'B'->'A'
         Weight[e.to.name][e.from.name] = e.weight;
      }
      return Weight;
   }

   //read in all the node and path-related data from CSV files
   public void readWaypointsFromCSV()
   {
      try
      {
         //Initialize variables
         segments = new int[136][4];
         BufferedReader br = new BufferedReader(new FileReader(Environment.getExternalStorageDirectory() + "/segments.csv"));
         BufferedReader br2 = new BufferedReader(new FileReader(Environment.getExternalStorageDirectory() + "/waypoints.csv"));
         BufferedReader br3 = new BufferedReader(new FileReader(Environment.getExternalStorageDirectory() + "/buildingentrances.csv"));
         StringTokenizer st = null;
         String inputLine = "";
         int line = 0;
         while((inputLine = br.readLine()) != null)
         {
            //parse tokens to read in endpoints of all segments from 'segments.csv'
            st = new StringTokenizer(inputLine, ",");
            segments[line][0] = Integer.parseInt(st.nextToken());
            segments[line][1] = Integer.parseInt(st.nextToken());
            segments[line][2] = Integer.parseInt(st.nextToken());
            segments[line][3] = Integer.parseInt(st.nextToken());
            line++;
         }
         line = 0;
         nodes = new ArrayList<Node>();
         while((inputLine = br2.readLine()) != null)
         {
            //parse tokens to read in node data from 'waypoints.csv'
            st = new StringTokenizer(inputLine, ",");
            nodes.add(new Node(Integer.parseInt(st.nextToken()), Integer.parseInt(st.nextToken()), line++));
         }
         buildingEntrances = new HashMap<String, Integer>();
         while((inputLine = br3.readLine()) != null)
         {
            //parse tokens to read in building entraces points and names
            //from 'buildingEntrances.csv'
            st = new StringTokenizer(inputLine, ",");
            int x = Integer.parseInt(st.nextToken());
            int y = Integer.parseInt(st.nextToken());
            String str = st.nextToken();
            for(int i = 0; i < nodes.size(); i++)
            {
               if (nodes.get(i).equalTo(x, y))
               {
                  buildingEntrances.put(str, i);
                  System.out.println(str + " @ " + i);
               }
            }
         }
      }
      catch (Exception e)
      {
         e.printStackTrace();
      }
   }

   //pulls the coordinates for buildingEntrances
   //by calling the HashMap with the building name as the hask key
   public int getBuildingEntrance(String str)
   {
      return buildingEntrances.get(str);
   }

   //returns the index of the node closest
   //to the given <x,y> position
   public int[] nearestNode(int x, int y)
   {
      int dist = 10000;
      int closest = 0;
      for(int i = 0; i < nodes.size(); i++)
      {
         int newDist = (int) Math.sqrt(Math.pow(nodes.get(i).x - x, 2) + Math.pow(nodes.get(i).y - y, 2));
         if (newDist < dist)
         {
            dist = newDist;
            closest = i;
         }
      }
      return new int[]{dist, closest};
   }
}

//wrapper class Node
//position and name information
//as well as a Node comparison function
class Node
{
   int name;
   int x, y;

   public Node(int x, int y, int argName)
   {
      name = argName;
      this.x = x;
      this.y = y;
   }

   public Node()
   {
   }

   public boolean equalTo(Node node1)
   {
      return (x == node1.x && y == node1.y);
   }

   public boolean equalTo(int x1, int y1)
   {
      return (x == x1 && y == y1);
   }
}

//wrapper class Edge
//contains information about which Nodes start
//and end the line segment.  also contains the edge distance or 'weight'
class Edge
{
   Node from, to;
   int weight;

   public Edge(Node argFrom, Node argTo, int argWeight)
   {
      from = argFrom;
      to = argTo;
      weight = argWeight;
   }

   public Edge()
   {
   }
}